/************************************************
 *** Datei: Keyboard.cpp
 ***
 *** Erstellt am: 29.05.2012
 ***       Autor: Max Stark <Max.Stark88@googlemail.com>
 ***	 Version: 1.0
 ***
 *** Beschreibung:
 *** <hier Beschreibung einfuegen>
 ************************************************/

#include "Keyboard.h"

Keyboard::Keyboard() {
	this->nextDroneCommand = new DroneCommand(0.0, 0.0, 0.0, 0.0, DRONE_SPECIAL_COMMAND_NONE);
	this->lastDroneCommand = new DroneCommand(0.0, 0.0, 0.0, 0.0, DRONE_SPECIAL_COMMAND_NONE);
	this->roll = 0.0;
	this->pitch = 0.0;
	this->yaw = 0.0;
	this->up = 0.0;
	this->isFlying = false;
	this->specialCommand = false;
	this->emergency = false;
	this->running = true;
	}

/*
 * Sets new build command as last command and creates new command to be set
 */
void Keyboard::makeNewCommand() {
	delete this->lastDroneCommand;
	this->lastDroneCommand = this->nextDroneCommand;
	this->nextDroneCommand = new DroneCommand(0.0, 0.0, 0.0, 0.0, DRONE_SPECIAL_COMMAND_NONE);
	specialCommand = false;
	Sleep(KEYBOARD_COMMAND_REFRESH_INTERVAL);
}


DroneCommand* Keyboard::getNextDroneCommand() {
	DroneCommand* dc = new DroneCommand(
			this->lastDroneCommand->getRoll(),
			this->lastDroneCommand->getPitch(),
			this->lastDroneCommand->getYaw(),
			this->lastDroneCommand->getVertical(),
			this->lastDroneCommand->getSpecialCommand()
		);

	return dc; //new DroneCommand(*(this->lastDroneCommand));
}

void Keyboard::run() {
	this->buildCommandsThread_Active = true;
	this->buildCommandsThread = new thread(
				&Keyboard::buildCommands, this);
}

/*
 * Build command with keyboard input
 */
void Keyboard::buildCommands() {

	while (this->buildCommandsThread_Active)
	{
		if(kbhit()) {
		  int c = getch();
		  //printf("Character: %d \n", c);

		  switch(c){
		  // case escape
		  case 27:
			  this->specialCommand = true;
			  this->nextDroneCommand->setSpecialCommand(DRONE_SPECIAL_COMMAND_EMERGENCY);
			  break;
			// Start / Land
		  case 101: //32: // ' '
			  specialCommand = true;
			  cout << "starten/landen" << endl;
			  if(this->isFlying){
				  this->nextDroneCommand->setSpecialCommand(DRONE_SPECIAL_COMMAND_LAND);
				  this->isFlying = false;
			  } else {
				  this->nextDroneCommand->setSpecialCommand(DRONE_SPECIAL_COMMAND_START);
				  this->isFlying = true;
			  }
			  break;
		  // Yaw
			  //a
		  case 97:
			  this->yaw += MOVE_N;
			  break;
			  // d
		  case 100:
			  this->yaw += MOVE_P;
			  break;
		  // Pitch
			  // k
		  case 107:
			  this->pitch += MOVE_N;
			  break;
			  // i
		  case 105:
			  this->pitch += MOVE_P;
			  break;
		  // Roll
			  // j
		  case 106:
			  this->roll += MOVE_P ;
			  break;
			  // l
		  case 108:
			  this->roll += MOVE_N ;
			  break;
		  // Up
			  // w
		  case 119:
			  this->up += MOVE_P;
			  break;
			  // s
		  case 115:
			  this->up += MOVE_N;
			  break;

			default:
			  break;
		   }
		}
		// Check all relevant Keystates which key is pressed
		short hiBit = (1 << 15);
		// Esc
		if(GetKeyState(27) & hiBit) {
		  this->specialCommand = true;
		  this->emergency = true;
		  this->nextDroneCommand->setSpecialCommand(DRONE_SPECIAL_COMMAND_EMERGENCY);
		} else if (this->emergency) {
			running = false;
		}
		// YAW
		// 97 ; a
		if(GetKeyState(0x41) & hiBit) {
			this->yaw += MOVE_N;
		} else
		// 100 ; d
		if( GetKeyState(0x44) & hiBit) {
			this->yaw += MOVE_P;
		} else {
			this->yaw = 0.0;
		}

		// PITCH
		// 107 ; k
		if( GetKeyState(0x4B) & hiBit) {
			this->pitch += MOVE_N;
		} else
		// 105 ; i
		if( GetKeyState(0x49) & hiBit) {
			this->pitch += MOVE_P;
		} else {
			this->pitch = 0.0;
		}

		// ROLL
		// 106 ; j
		if(GetKeyState(0x4A) & hiBit) {
			this->roll += MOVE_P ;
		} else
		// 108 ; l
		if( GetKeyState(0x4C) & hiBit) {
			this->roll += MOVE_N ;
		} else {
			this->roll = 0.0;
		}

		// UP
		// 119; w
		if(GetKeyState(0x57) & hiBit) {
			this->up += MOVE_P;
		} else
		// 115; s
		if(GetKeyState(0x53) & hiBit) {
			this->up += MOVE_N;
		} else {
			this->up = 0.0;
		}
		// set values to command
		if (yaw == 0.0 and pitch == 0.0 and roll == 0.0 and up == 0.0 and !(this->specialCommand)) {
			this->nextDroneCommand->setSpecialCommand(DRONE_SPECIAL_COMMAND_HOVER);
		} else {
		  this->nextDroneCommand->setSpecialCommand(DRONE_SPECIAL_COMMAND_NONE);
		  this->nextDroneCommand->setYaw(yaw);
		  this->nextDroneCommand->setPitch(pitch);
		  this->nextDroneCommand->setRoll(roll);
		  this->nextDroneCommand->setVertical(up);
		}
		//this->nextDroneCommand->printToConsole();
		this->makeNewCommand();
		Sleep(KEYBOARD_COMMAND_BUILD_INTERVAL);
	}
}

string Keyboard::getDeviceName() {
	return "WindowsKeyboard";
}

Keyboard::~Keyboard() {
	this->buildCommandsThread_Active = false;
	this->buildCommandsThread->join();
	delete this->buildCommandsThread;

	free(this->nextDroneCommand);
}

